/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2022-03-18     desktop       the first version
 */

#include <all_code.h>

/*  导入log工具 */
#define DBG_TAG "hand.c"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define hand_pwm_speed 25
/*  手部线程控制块 */
static rt_thread_t hand_scheduling_tid = RT_NULL;

/* 手部调度信号量 */
struct rt_semaphore hand_sch_semaphore;

rt_uint8_t hand_scheduling_flag = 0;
//前面两个线程将自动接收数据，这里要创建一个线程来完成任务调度
rt_uint32_t HAND_PWM_MAX = 0;
rt_uint32_t HAND_PWM_MIN = 0;
//电机要完成正转，停止，反转，停止。通过标志位去控制，这几个函数只调用一次
//任务调度函数通过信号量一直循环

static void hand_scheduling_entry(void *param)
{
    //angle_hand_pwm 是转速，now_hand_pwm 是当前PWM值
    rt_uint32_t now_hand_pwm=0;
    now_hand_pwm = HAND_PWM_MIN;
    pwm_hand_set(now_hand_pwm);
    while(1)
    {
        //说明有数据变化，来查看一次数据调度
        rt_sem_take(&hand_sch_semaphore, RT_WAITING_FOREVER);
        //LOG_I("接收到手部信号量\t\n");
        //如果开关是打开的，判断flag数值
        if(temp_data[2] == 1)
        {
            if(hand_scheduling_flag == 0)
            {
                //如果标志位是0，且开关打开，就使能定时器50ms，置位flag=1
                //打开定时器，固定时间延迟，在中断将 标志位++，这边来 设置工作模式

                //获取当前角度的最大值和最小值，并置于最小值上
                get_hand_pwm_pulse(temp_data[1]);
                now_hand_pwm = HAND_PWM_MIN;
                pwm_hand_set(now_hand_pwm);
                //获取pwm角度，即最大MAX和最小MIN
                //设置定时器中断时间
                hand_timeout_s.sec = 0; /* 0s */
                hand_timeout_s.usec = 50000; /* 50ms */
                //开始定时器
                rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
                LOG_I("启动手部硬件定时器\t\n");
                hand_scheduling_flag = 1;
            }

            //从最小正转到最大，最小和最大由 协议里获取
            //如果 hand_scheduling_flag 是1， 在中断里要不断的释放信号量
            if(hand_scheduling_flag ==1 && temp_data[2] ==1)
            {
                //转
                if(now_hand_pwm < HAND_PWM_MAX)
                {
                    now_hand_pwm += hand_pwm_speed;
                    pwm_hand_set(now_hand_pwm);
                    //LOG_I("手部正转\t\n");
                }
                //转到位停止
                else if(now_hand_pwm >= HAND_PWM_MAX)
                {
                    now_hand_pwm = HAND_PWM_MAX;
                    pwm_hand_set(now_hand_pwm);
                    //到这里转到位开始停止，把定时器设置成1s，1s后中断触发就说明已经停够1s了
                    //然后在中断里把flag变成3，说明准备反转了
                    //设置定时器中断时间
                    hand_timeout_s.sec = 3; /* 1s */
                    hand_timeout_s.usec = 0; /* 0ms */
                    rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
                    hand_scheduling_flag = 2;
                    LOG_E("手部开始第一次停止\t\n");
                }
            }

            //到这里就准备配置定时器然后反转，配置完成后flag就是4了
            else if(hand_scheduling_flag ==3 && temp_data[2] ==1)
            {
                //将中断变为50ms
                hand_timeout_s.sec = 0;
                hand_timeout_s.usec = 50000; /* 50ms */
                rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
                hand_scheduling_flag = 4;
            }


            // flag等于4就是反转，转到了就把flag变成5
            else if(hand_scheduling_flag == 4 && temp_data[2] ==1)
            {
                //转
                if(now_hand_pwm > HAND_PWM_MIN)
                {
                    now_hand_pwm -= hand_pwm_speed;
                    pwm_hand_set(now_hand_pwm);
                    //LOG_I("手部反转\t\n");
                }
                //转到位停止
                else if(now_hand_pwm <= HAND_PWM_MIN)
                {
                    now_hand_pwm = HAND_PWM_MIN;
                    pwm_hand_set(HAND_PWM_MIN);
                    //这里把flag变成5把定时器设置成1s，
                    hand_timeout_s.sec = 3; /* 1s */
                    hand_timeout_s.usec = 0; /* 50ms */
                    rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
                    hand_scheduling_flag = 5;
                    LOG_E("手部开始第二次停止\t\n");
                }
            }

            //1s后就已经停止完成了，然后再回到flag=1
            else if(hand_scheduling_flag == 6 && temp_data[2] ==1)
            {
                hand_scheduling_flag = 1;
                hand_timeout_s.sec = 0; /* 0s */
                hand_timeout_s.usec = 50000; /* 50ms */
                rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
            }

        }


        //如果开关是关着的，判断flag数值
        else if(temp_data[2] == 0)
        {
                //判断一下当前的位置是不是最小点，如果是最小点就把flag置0，
                //如果不是最小点就要转回去
            if(now_hand_pwm == HAND_PWM_MIN || hand_scheduling_flag == 8)
            {
                rt_device_control(hand_hw_time_dev,HWTIMER_CTRL_STOP,&freq);
                hand_scheduling_flag = 0;
                LOG_E("手部关机复位\t\n");
            }

            //如果不是最小点，就把flag变成5，定时器50ms
            else if(now_hand_pwm != HAND_PWM_MIN && hand_scheduling_flag != 7)
            {
                hand_scheduling_flag = 7;
                hand_timeout_s.sec = 0; /* 0s */
                hand_timeout_s.usec = 50000; /* 50ms */
                rt_device_write(hand_hw_time_dev, 0, &hand_timeout_s, sizeof(hand_timeout_s));
            }

            if(hand_scheduling_flag == 7 && temp_data[2] == 0)
            {
                if(now_hand_pwm > HAND_PWM_MIN)
                {
                    now_hand_pwm -= hand_pwm_speed;
                    pwm_hand_set(now_hand_pwm);
                    //LOG_E("手部关机复位\t\n");
                }

                else if(now_hand_pwm <= HAND_PWM_MIN)
                {

                    pwm_hand_set(HAND_PWM_MIN);
                    hand_scheduling_flag = 8;
                }
            }



        }

    }
}


void hand_app_creative(void)
{
    //创建信号量
    rt_sem_init(&hand_sch_semaphore, "hand_sch_semaphore", 0, RT_IPC_FLAG_PRIO);

    //创建手部调度线程并启动
    hand_scheduling_tid = rt_thread_create("hand_sch", hand_scheduling_entry, RT_NULL, 512, 14, 10);
    if(hand_scheduling_tid != RT_NULL)
    {
        rt_thread_startup(hand_scheduling_tid);
    }
}
